/*
需要配置（带星号*为必须配置）
宏定义 
SLAVE_ADDRESS  地址      (*)
RIGHT_DIR      安装方向  (*)

LIDAR_ABOVE_RANGE 雷达上方避障高度
LIDAR_BELOW_RANGE 雷达下方避障高度
DIS_LIMIT         最大测量限制
*/
// This sketch code is based on the RPLIDAR driver library provided by RoboPeak
#include <RPLidar.h>
#include <Wire.h>

// You need to create an driver instance 
RPLidar lidar;

#define LIDAR_ABOVE_RANGE (300.0)
#define LIDAR_BELOW_RANGE (100.0)
#define DIS_LIMIT         (1000.0)

#define ANG2RAD(x) ((x) * (0.01745329252))

#define RPLIDAR_MOTOR 3 // The PWM pin for control the speed of RPLIDAR's motor.
                        // This pin should connected with the RPLIDAR's MOTOCTRL signal 

//i2c从机地址
#define SLAVE_ADDRESS (0xb0 >> 1)

//定义安装方向（正对雷达，观察的雷达方向为准）
//#define RIGHT_DIR

#ifdef RIGHT_DIR
const float LIDAR_BELOW_ANGLES[2] = {270.1, 360.0};
const float LIDAR_ABOVE_ANGLES[2] = {180.0, 270.0}; 
#else
const float LIDAR_ABOVE_ANGLES[2] = {90.0, 180.0};
const float LIDAR_BELOW_ANGLES[2] = {0.0, 89.9};
#endif

float feedback_dis = 0.0;
float min_dis_per_frame = 0.0;
float angle_g = 0.0;
void receiveEvent(int howMany) {
}

void requestEvent() {
  int16_t dis = (int16_t)min_dis_per_frame;
//  if (feedback_dis > 0.0) {
//    Serial.print(angle);
//    Serial.print("\t");
//    Serial.print(distance);
//    Serial.print("\t");
//    Serial.print(quality);
//    Serial.print("\t");
//    Serial.println(feedback_dis);
//  }

  while (Wire.available()) {
    char c = Wire.read();
//    Serial.print(millis());
    if (dis > 0) {
//      Serial.print("\t:");
      Serial.println(dis);
    }
//    Serial.print("\t:");
//    Serial.println((int8_t)(dis >> 8) & 0xff, HEX);
//    Serial.print("\t:");
//    Serial.println((int8_t)(dis & 0xff), HEX);
    if (c == 0x02) {                              //返回距离高8位
      Wire.write((int8_t)(dis >> 8) & 0xff);
    }
    if (c == 0x03) {                              //返回距离低8位
      Wire.write((int8_t)(dis & 0xff));
    }
  }
}

bool inAngleRange(float angle) {
  if ((angle > LIDAR_ABOVE_ANGLES[0] && angle < LIDAR_ABOVE_ANGLES[1])
    || (angle > LIDAR_BELOW_ANGLES[0] && angle < LIDAR_BELOW_ANGLES[1]))
    return true;

  return false;
}

#define MAX_DIS (100000.0f)
#define MIN_DIS (0.000001f)

float distance_array[100];
float angle_array[100];
int distance_array_pos = 0;

bool isValidDistance(float dis) {
  if (dis > 150.0f) return true;

  return false;
}

bool isEmpty() {
  return distance_array_pos == 0;
}

bool isAllZero() {
  for (int i = 0; i < distance_array_pos; i ++) {
    if (distance_array[i] > MIN_DIS) return false;
  }
  return true;
}

float getMinDistance() {
  float dis = MAX_DIS;
  for (int i = 0; i < distance_array_pos; i ++) {
    if (distance_array[i] > MIN_DIS) {
      if (distance_array[i] < dis) {
        dis = distance_array[i];
      }
    }
  }

  return dis;
}

float getRoundDistance() {
  if (isEmpty()) return 0.0f;

  if (isAllZero()) return 0.0f;

  return getMinDistance();
}

void setup() {
  // bind the RPLIDAR driver to the arduino hardware serial
  lidar.begin(Serial1);
  Serial.begin(115200);
  // set pin modes
//  pinMode(RPLIDAR_MOTOR, OUTPUT);

  memset(distance_array, 0.0, sizeof(distance_array));
  memset(angle_array, 0.0, sizeof(angle_array));

  Wire.begin(SLAVE_ADDRESS);                // join i2c bus with address #8
  Wire.onReceive(receiveEvent); // register event
  Wire.onRequest(requestEvent); // register event
}


//bool new_scan = false;

long time_last = 0.0;

void loop() {
  float vertical_range;
  if (IS_OK(lidar.waitPoint())) {
    float distance = lidar.getCurrentPoint().distance; //distance value in mm unit
    float angle    = lidar.getCurrentPoint().angle; //anglue value in degree
    bool  startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan
    byte  quality  = lidar.getCurrentPoint().quality; //quality of the current measurement

    
    if (startBit) {
//      Serial.println("startBit");
      
//      if(inAngleRange(angle)) min_dis_per_frame = 0.0;//新的数据帧，重置距离
//      Serial.print("\t array = ");
//      for (int i = 0; i < distance_array_pos; i ++) {
//        Serial.print(distance_array[i]);
//        Serial.print("\t");
//      }
//      Serial.print("\n");
//
//      Serial.print("\t angle = ");
//      for (int i = 0; i < distance_array_pos; i ++) {
//        Serial.print(angle_array[i]);
//        Serial.print("\t");
//      }
//      Serial.print("\n");      
      min_dis_per_frame = getRoundDistance();
//      Serial.print("getRoundDistance =\t");
//      Serial.println(min_dis_per_frame);
//      Serial.print("millis =\t");
//      long time_now = millis();
//      Serial.println(time_now - time_last);
//      time_last = time_now;
      
      memset(distance_array, 0.0, sizeof(distance_array));
      memset(angle_array, 0.0, sizeof(angle_array));
      
      distance_array_pos = 0;
    }
    if (angle > LIDAR_ABOVE_ANGLES[0] && angle < LIDAR_ABOVE_ANGLES[1])
      vertical_range = LIDAR_ABOVE_RANGE;
    if (angle > LIDAR_BELOW_ANGLES[0] && angle < LIDAR_BELOW_ANGLES[1])
      vertical_range = LIDAR_BELOW_RANGE;
          
    float radian = ANG2RAD(angle);
    if (inAngleRange(angle)) {
      if ((distance < (vertical_range / fabs(cos(radian)))) && distance <= DIS_LIMIT && isValidDistance(distance)) {
        float ret = distance * sin(radian);
        distance_array[distance_array_pos] = fabs(ret);
        angle_array[distance_array_pos] = angle;
        distance_array_pos ++;
//        float ret = distance * cos(radian);
//        min_dis_per_frame = ret;
        if (ret > 0.0) {
//          if (min_dis_per_frame == 0.0)
//          min_dis_per_frame = ret;
//          if (ret < min_dis_per_frame) {
//            min_dis_per_frame = ret;
//          }
        }
      }
    }

//    if (inAngleRange(angle)) {
//      if (min_dis_per_frame <= 128.0) min_dis_per_frame = 0.0;
//          if (true) {
//      Serial.print(angle);
//      Serial.print("\t");
//      Serial.print(distance);
//      Serial.print("\t");
//      Serial.print(quality);
//      Serial.print("\t");
//      Serial.println(min_dis_per_frame);
//    }
//    }

//    feedback_dis = min_dis_per_frame;
//    if (angle >=270 && angle < 360) {
//      float radian = ANG2RAD(angle);
//      if ((distance < (LIDAR_ABOVE_RANGE / fabs(sin(radian)))) && distance <= DIS_LIMIT) {
//        float ret = distance * cos(radian);
//        if (ret > 0.0) {
//           if (min_dis_per_frame == 0.0)
//             min_dis_per_frame = ret;
//          if (ret < min_dis_per_frame) {
//            min_dis_per_frame = ret;
//          }
//        }
//      }
//    }
    
  } else {
    min_dis_per_frame = 0.0;
//    analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor
    
    // try to detect RPLIDAR... 
    rplidar_response_device_info_t info;
    if (IS_OK(lidar.getDeviceInfo(info, 100))) {
       // detected...
       lidar.startScan();
       
       // start motor rotating at max allowed speed
//       analogWrite(RPLIDAR_MOTOR, 210);
       delay(1000);
    }
  }
}
